#include <cmath>
#include <string>
#include <GL/gl.h>
#include <GL/glu.h>

#include <SRL/Utils/Color.h>
#include <SRL/Utils/Vector.h>
#include <SRL/Utils/Matrix.h>
#include <SRL/Utils/BoundingBox.h>
#include <SRL/Engine/Camera.h>
#include <SRL/Engine/Frustum.h>

namespace srl {
    /**************** Plane *******************/
    
    // Constructor
    Plane::Plane(){}
    Plane::Plane(srl::Vector<float> pNormale) { Set(pNormale); }
    Plane::Plane(float pa, float pb, float pc, float pd) { Set(pa, pb, pc, pd); }
    
    // Set functions
    void Plane::Set(srl::Vector<float> pNormale) {
        Normale = pNormale;
    }
    void Plane::Set(float pa, float pb, float pc, float pd) {
        Normale = srl::Vector<float>(pa, pb, pc, pd);
    }
    
    // Tests functions
    bool Plane::IsOn(srl::Vector<float> Point) {
        return (Normale.x*Point.x + Normale.y*Point.y + Normale.z*Point.z + Normale.w == 0);
    }
    bool Plane::IsOver(srl::Vector<float> Point) {
        return (Normale.x*Point.x + Normale.y*Point.y + Normale.z*Point.z + Normale.w > 0);
    }
    bool Plane::IsUnder(srl::Vector<float> Point) {
        return (Normale.x*Point.x + Normale.y*Point.y + Normale.z*Point.z + Normale.w < 0);
    }
    
    // Normalize the plane
    void Plane::Normalize() {
        Normale.Normalize();
        Normale.w /= Normale.Length();   
    }
    
    /************** Frustum *******************/
    
    // Constructor
    Frustum::Frustum() : Camera(0) {}
    // Constructor - Save a pointer to a camera
    Frustum::Frustum(srl::Camera *pCamera) : Camera(pCamera) { Make(); }
    
    // Make with the pointer ginen in instanciation
    void Frustum::Make() {
        /*Planes[0].Set();
        Planes[1].Set();
        Planes[2].Set();
        Planes[3].Set();
        Planes[4].Set();
        Planes[5].Set();*/
    }
    
    // Make planes with a given camera
    void Frustum::Make(srl::Camera *pCamera) {
        /*Planes[0].Set();
        Planes[1].Set();
        Planes[2].Set();
        Planes[3].Set();
        Planes[4].Set();
        Planes[5].Set();*/
    }
    
    // Make planes with the openGL matixes
    void Frustum::MakeGL() {
        srl::Matrix<float> Modelview, Projection;
        glGetFloatv(GL_MODELVIEW_MATRIX,  Modelview[0]  );
        glGetFloatv(GL_PROJECTION_MATRIX, Projection[0] );
        
        srl::Matrix<float> Final = Modelview * Projection;
        // Near
        Planes[0].Set( Final[1][4]+Final[1][3],  Final[2][4]+Final[2][3],  Final[3][4]+Final[3][3],   Final[4][4]+Final[4][3] );
        Planes[0].Normalize();
        // Far
        Planes[1].Set( Final[1][4]-Final[1][3],  Final[2][4]-Final[2][3],  Final[3][4]-Final[3][3],   Final[4][4]-Final[4][3] );
        Planes[1].Normalize();
        // Right
        Planes[2].Set( Final[1][4]-Final[1][1],  Final[2][4]-Final[2][1],  Final[3][4]-Final[3][1],    Final[4][4]-Final[4][1] );
        Planes[2].Normalize();
        // Left
        Planes[3].Set( Final[1][4]+Final[1][1],  Final[2][4]+Final[2][1],  Final[3][4]+Final[3][1],    Final[4][4]+Final[4][1] );
        Planes[3].Normalize();
        // Top
        Planes[4].Set( Final[1][4]-Final[1][2],  Final[2][4]-Final[2][2],  Final[3][4]-Final[3][2],   Final[4][4]-Final[4][2] );
        Planes[4].Normalize();
        // Bottom
        Planes[5].Set( Final[1][4]+Final[1][2],  Final[2][4]+Final[2][2],  Final[3][4]+Final[3][2],   Final[4][4]+Final[4][2] );
        Planes[5].Normalize();
    }
    
    // Test a point
    bool Frustum::Test(srl::Vector<float> point) {
        /*for(int i = 0; i < 6; i++) {
            if(Planes[i].IsUnder(point)) return false;
        }*/
        if(Planes[4].IsUnder(point)) return false;
        return true;
    }
    
    // Test a bounding box
    bool Frustum::Test(srl::BoundingBox Box) {
        srl::Vector<float> Size = Box.GetSize();
        if( !Test( Box.Min ) && 
            !Test( srl::Vector<float>(Box.Min.x + Size.x, Box.Min.y, Box.Min.z) ) && 
            !Test( srl::Vector<float>(Box.Min.x, Box.Min.y + Size.y, Box.Min.z) ) && 
            !Test( srl::Vector<float>(Box.Min.x, Box.Min.y, Box.Min.z + Size.z) ) && 
            !Test( srl::Vector<float>(Box.Min.x + Size.x, Box.Min.y, Box.Min.z + Size.z) ) && 
            !Test( srl::Vector<float>(Box.Min.x + Size.x, Box.Min.y + Size.y, Box.Min.z) ) && 
            !Test( srl::Vector<float>(Box.Min.x, Box.Min.y + Size.y, Box.Min.z + Size.z) ) && 
            !Test( Box.Max ) )
            return false;
            
        return true;
    }
    
    // Get the instance
    srl::Frustum* Frustum::Get() {
        static srl::Frustum CurrentFrustum;
        return &CurrentFrustum;
    }
}
